import vue from '../main'
import store from '../store/index';
import { Notification } from 'element-ui';
import { OBJ, viewer } from './init'
import { debounce } from './methods'

/**
 * 绘制robot
 * 放大缩小时,地图加载时
 * 移除重复的
 * **/
export function robotMarker() {
    return new Promise(function (resolve, reject) {
        if (OBJ.robotStage) {
            viewer.scene.removeChild(OBJ.robotStage);
        }
        // marker for the robot
        var robotMarker = new ROS2D.NavigationArrow({
            size: 20,
            strokeSize: 1,
            fillColor: createjs.Graphics.getRGB(255, 255, 153, 0.66),
            pulse: true
        });
        // wait for a pose to come in first
        robotMarker.visible = false;
        viewer.scene.addChild(robotMarker);
        var initScaleSet = false;

        // setup a listener for the robot pose
        OBJ.topic.robotPoseTopic.subscribe(function (pose) {
            OBJ.robotPose = pose;
            // update the robots position on the map
            robotMarker.x = pose.position.x;
            robotMarker.y = -pose.position.y;
            if (!initScaleSet) {
                robotMarker.scaleX = 1.0 / viewer.scene.scaleX;
                robotMarker.scaleY = 1.0 / viewer.scene.scaleY;
                initScaleSet = true;
            }
            // change the angle
            robotMarker.rotation = viewer.scene.rosQuaternionToGlobalTheta(pose.orientation);
            robotMarker.visible = true;
            OBJ.robotStage = robotMarker;
            resolve()
        });
    })
}

//robot定位
export function locationRobot() {
    // zoomview.startZoom(OBJ.imgMapStage.currentImage.x + OBJ.robotStage.x, OBJ.imgMapStage.currentImage.y - OBJ.robotStage.y);
    viewer.scene.x = OBJ.mapXY.x + OBJ.robotStage.x;
    viewer.scene.y = OBJ.mapXY.y + OBJ.robotStage.y;
}

/**
 * 手动定位
 * switchMoveMap    地图禁止拖动
 * **/
export function poseEstimate(){
    Notification({
        title: vue.$t("titles.info"),
        message: vue.$t("messages.initialPoseTios"),
        type: 'info',
        position: 'top-left',
        offset: 100
    });

    store.commit("switchMoveMap", true);
    let startPose = null;
    let endPose = null;

    // 当鼠标与多边形交互回调函数
    var clickedPolygon = false;
    var selectedPointIndex = null;

    var pointCallBack = function(type, event, index) {
        if (type === 'mousedown') {
            if (event.nativeEvent.shiftKey === true) {
                polygon.remPoint(index);
            }
            else {
                selectedPointIndex = index;
            }
        }
        clickedPolygon = true;
    };

    var lineCallBack = function(type, event, index) {
        if (type === 'mousedown') {
            if (event.nativeEvent.ctrlKey === true) {
                polygon.splitLine(index);
            }
        }
        clickedPolygon = true;
    }

    // 创建多边形
    var polygon = new ROS2D.PolygonMarker({
        lineColor : createjs.Graphics.getRGB(100, 100, 255, 1),
        lineSize: 0.1,
        pointSize: 0.2,
        pointCallBack : pointCallBack,
        lineCallBack : lineCallBack
    });

    // 多边形添加到画布
    viewer.scene.addChild(polygon);

    // 鼠标事件监听
    viewer.scene.mouseMoveOutside = false; // 好像没用

    viewer.scene.addEventListener('stagemousemove', function(event) {
        // 拖动时移动点
        if (selectedPointIndex !== null) {
            var pos = viewer.scene.globalToRos(event.stageX, event.stageY);
            polygon.movePoint(selectedPointIndex, pos);
            
            if(selectedPointIndex == 0){
                startPose = viewer.scene.globalToRos(event.stageX, event.stageY);
            }else if(selectedPointIndex == 1){
                endPose = viewer.scene.globalToRos(event.stageX, event.stageY);
            }

            debounceSendPose({startPose, endPose, polygon});
        }
    });

    viewer.scene.addEventListener('stagemouseup', function(event) {
        //获取点个数
        let numPoints = polygon.pointContainer.getNumChildren() + 1;
        
        // 未点击多边形时添加点
        if (selectedPointIndex !== null) {
            selectedPointIndex = null;
        }

        if (viewer.scene.mouseInBounds === true && clickedPolygon === false) {
            if(numPoints == 1) {//第一个点坐标
                startPose = viewer.scene.globalToRos(event.stageX, event.stageY);
            }else if(numPoints == 2){//第二个点坐标
                endPose = viewer.scene.globalToRos(event.stageX, event.stageY);
                debounceSendPose({startPose, endPose, polygon});
            }else{
                //最多两个点
                return
            }
            var pos = viewer.scene.globalToRos(event.stageX, event.stageY);
            polygon.addPoint(pos);
        }
        clickedPolygon = false;
    });
}

/**
 * 防抖
 * 发送手动定位
 * startPose    开始坐标
 * endPose      结束坐标
 * polygon      点线对象
 * **/
let debounceSendPose = debounce((params)=>{
    let {startPose, endPose, polygon} = params;
    OBJ.initialPoseStage = polygon;
    let xDelta = startPose.x - endPose.x;
    let yDelta = startPose.y - endPose.y;
    let thetaRadians = Math.atan2(xDelta, yDelta);
    if (thetaRadians >= 0 && thetaRadians <= Math.PI) {
        thetaRadians += (3 * Math.PI / 2);
    } else {
        thetaRadians -= (Math.PI / 2);
    }

    thetaRadians += Math.PI;
    let qz = Math.sin(-thetaRadians / 2.0);
    let qw = Math.cos(-thetaRadians / 2.0);

    let orientation = new ROSLIB.Quaternion({ x: 0, y: 0, z: qz, w: qw });

    var covariance = [];
    for (var i = 0; i < 36; i++){
        covariance[i] = 0;
    }
    covariance[0] = 0.25;
    covariance[7] = 0.25;
    covariance[35] = Math.pow((Math.PI/12), 2);
    let poseMsg =new ROSLIB.Message({
        header:{
            seq: 0,
            frame_id: "map"
        },
        pose:{
            pose:{
                position: startPose,
                orientation
            },
            covariance
        }
    })
    OBJ.topic.initialPoseTopic.publish(poseMsg)
    removeInitialPoseStage();
}, 500)

/**
 * 防抖
 * 5S之后移除点线图标
 * **/
let removeInitialPoseStage = debounce(()=>{
    viewer.scene.removeChild(OBJ.initialPoseStage);
    store.commit("switchMoveMap", false);
}, 5000)